#include "des.h"

/**
 * @brief f 函数
 *
 */
static void F(uint8_t *pDataIn, const uint8_t *pKi);

/**
 * @brief S 盒代替
 *
 */
static void SBox(uint8_t *pDataOut, const uint8_t *pDataIn);

/**
 * @brief 变换
 *
 */
static void Transform(uint8_t *pDataOut, uint8_t *pDataIn, const uint8_t *Table, uint16_t Length);

/**
 * @brief 循环左移
 *
 */
static void RotateL(uint8_t *pDataIn, uint16_t Length, uint16_t loop);

/**
 * @brief 字节组转换成位组
 *
 */
static void ByteToBit(uint8_t *pDataOut, const uint8_t *pDataIn, uint16_t Bits);

/**
 * @brief 位组转换成字节组
 *
 */
static void BitToByte(uint8_t *pDataOut, const uint8_t *pDataIn, uint16_t Bits);

/**
 * @brief 异或计算
 *
 */
static void Xor(uint8_t *InA, const uint8_t *InB, uint16_t Length);

/**
 * @brief 异或计算
 *
 * @param pDataIn  - 源数据
 * @param pDest    - 目标异或数值
 * @param DataSize - 数据有效长度
 * @param pDataOut - 异或后的数据
 */
static void DataXor(uint8_t *pDataIn, uint8_t *pDest, uint32_t DataSize, uint8_t *pDataOut);

/**
 * @brief 设置密钥
 *
 * @param pKeyIn - DES 密钥
 */
static void DesSetKey(const uint8_t *pKeyIn);

/**
 * @brief DES 运算
 *
 */
static void Des(uint8_t *pDataOut, const uint8_t *pDataIn, uint8_t Type);

// 左移位数表
const uint8_t LoopTable[16] = {1, 1, 2, 2, 2, 2, 2, 2, 1, 2, 2, 2, 2, 2, 2, 1};

// PC1选位表(子密钥生成置换表1)
const uint8_t PC1_Table[56] = {
    57, 49, 41, 33, 25, 17, 9, 1, 58, 50, 42, 34, 26, 18,
    10, 2, 59, 51, 43, 35, 27, 19, 11, 3, 60, 52, 44, 36,
    63, 55, 47, 39, 31, 23, 15, 7, 62, 54, 46, 38, 30, 22,
    14, 6, 61, 53, 45, 37, 29, 21, 13, 5, 28, 20, 12, 4};

// PC2选位表(子密钥生成置换表2) 生成子密钥 48位
const uint8_t PC2_Table[48] = {
    14, 17, 11, 24, 1, 5, 3, 28, 15, 6, 21, 10,
    23, 19, 12, 4, 26, 8, 16, 7, 27, 20, 13, 2,
    41, 52, 31, 37, 47, 55, 30, 40, 51, 45, 33, 48,
    44, 49, 39, 56, 34, 53, 46, 42, 50, 36, 29, 32};

// 置换IP表
const uint8_t IP_Table[64] = {
    58, 50, 42, 34, 26, 18, 10, 2, 60, 52, 44, 36, 28, 20, 12, 4,
    62, 54, 46, 38, 30, 22, 14, 6, 64, 56, 48, 40, 32, 24, 16, 8,
    57, 49, 41, 33, 25, 17, 9, 1, 59, 51, 43, 35, 27, 19, 11, 3,
    61, 53, 45, 37, 29, 21, 13, 5, 63, 55, 47, 39, 31, 23, 15, 7};

// 逆置换IP-1表
const uint8_t IPR_Table[64] = {
    40, 8, 48, 16, 56, 24, 64, 32, 39, 7, 47, 15, 55, 23, 63, 31,
    38, 6, 46, 14, 54, 22, 62, 30, 37, 5, 45, 13, 53, 21, 61, 29,
    36, 4, 44, 12, 52, 20, 60, 28, 35, 3, 43, 11, 51, 19, 59, 27,
    34, 2, 42, 10, 50, 18, 58, 26, 33, 1, 41, 9, 49, 17, 57, 25};

// E位选择表(扩展置换表)
const uint8_t E_Table[48] = {
    32, 1, 2, 3, 4, 5, 4, 5, 6, 7, 8, 9,
    8, 9, 10, 11, 12, 13, 12, 13, 14, 15, 16, 17,
    16, 17, 18, 19, 20, 21, 20, 21, 22, 23, 24, 25,
    24, 25, 26, 27, 28, 29, 28, 29, 30, 31, 32, 1};

// P换位表(单纯换位表)
const uint8_t P_Table[32] = {
    16, 7, 20, 21, 29, 12, 28, 17, 1, 15, 23, 26, 5, 18, 31, 10,
    2, 8, 24, 14, 32, 27, 3, 9, 19, 13, 30, 6, 22, 11, 4, 25};

// S盒
const uint8_t S_Box[8][4][16] = {
    {
        {14, 4, 13, 1, 2, 15, 11, 8, 3, 10, 6, 12, 5, 9, 0, 7},
        {0, 15, 7, 4, 14, 2, 13, 1, 10, 6, 12, 11, 9, 5, 3, 8},
        {4, 1, 14, 8, 13, 6, 2, 11, 15, 12, 9, 7, 3, 10, 5, 0},
        {15, 12, 8, 2, 4, 9, 1, 7, 5, 11, 3, 14, 10, 0, 6, 13},
    },

    {
        {15, 1, 8, 14, 6, 11, 3, 4, 9, 7, 2, 13, 12, 0, 5, 10},
        {3, 13, 4, 7, 15, 2, 8, 14, 12, 0, 1, 10, 6, 9, 11, 5},
        {0, 14, 7, 11, 10, 4, 13, 1, 5, 8, 12, 6, 9, 3, 2, 15},
        {13, 8, 10, 1, 3, 15, 4, 2, 11, 6, 7, 12, 0, 5, 14, 9},
    },

    {
        {10, 0, 9, 14, 6, 3, 15, 5, 1, 13, 12, 7, 11, 4, 2, 8},
        {13, 7, 0, 9, 3, 4, 6, 10, 2, 8, 5, 14, 12, 11, 15, 1},
        {13, 6, 4, 9, 8, 15, 3, 0, 11, 1, 2, 12, 5, 10, 14, 7},
        {1, 10, 13, 0, 6, 9, 8, 7, 4, 15, 14, 3, 11, 5, 2, 12},
    },

    {
        {7, 13, 14, 3, 0, 6, 9, 10, 1, 2, 8, 5, 11, 12, 4, 15},
        {13, 8, 11, 5, 6, 15, 0, 3, 4, 7, 2, 12, 1, 10, 14, 9},
        {10, 6, 9, 0, 12, 11, 7, 13, 15, 1, 3, 14, 5, 2, 8, 4},
        {3, 15, 0, 6, 10, 1, 13, 8, 9, 4, 5, 11, 12, 7, 2, 14},
    },

    {
        {2, 12, 4, 1, 7, 10, 11, 6, 8, 5, 3, 15, 13, 0, 14, 9},
        {14, 11, 2, 12, 4, 7, 13, 1, 5, 0, 15, 10, 3, 9, 8, 6},
        {4, 2, 1, 11, 10, 13, 7, 8, 15, 9, 12, 5, 6, 3, 0, 14},
        {11, 8, 12, 7, 1, 14, 2, 13, 6, 15, 0, 9, 10, 4, 5, 3},
    },

    {
        {12, 1, 10, 15, 9, 2, 6, 8, 0, 13, 3, 4, 14, 7, 5, 11},
        {10, 15, 4, 2, 7, 12, 9, 5, 6, 1, 13, 14, 0, 11, 3, 8},
        {9, 14, 15, 5, 2, 8, 12, 3, 7, 0, 4, 10, 1, 13, 11, 6},
        {4, 3, 2, 12, 9, 5, 15, 10, 11, 14, 1, 7, 6, 0, 8, 13},
    },

    {
        {4, 11, 2, 14, 15, 0, 8, 13, 3, 12, 9, 7, 5, 10, 6, 1},
        {13, 0, 11, 7, 4, 9, 1, 10, 14, 3, 5, 12, 2, 15, 8, 6},
        {1, 4, 11, 13, 12, 3, 7, 14, 10, 15, 6, 8, 0, 5, 9, 2},
        {6, 11, 13, 8, 1, 4, 10, 7, 9, 5, 0, 15, 14, 2, 3, 12},
    },

    {
        {13, 2, 8, 4, 6, 15, 11, 1, 10, 9, 3, 14, 5, 0, 12, 7},
        {1, 15, 13, 8, 10, 3, 7, 4, 12, 5, 6, 11, 0, 14, 9, 2},
        {7, 11, 4, 1, 9, 12, 14, 2, 0, 6, 10, 13, 15, 3, 5, 8},
        {2, 1, 14, 7, 4, 10, 8, 13, 15, 12, 9, 0, 3, 5, 6, 11},
    },
};

static uint8_t SubKey[16][48] = {0};

static void F(uint8_t *pDataIn, const uint8_t *pKi)
{
  uint8_t MR[48] = {0};

  if ((pDataIn != NULL) && (pKi != NULL))
  {
    Transform(MR, pDataIn, E_Table, 48);
    Xor(MR, pKi, 48);
    SBox(pDataIn, MR);
    Transform(pDataIn, pDataIn, P_Table, 32);
  }
}

static void SBox(uint8_t *pDataOut, const uint8_t *pDataIn)
{
  if ((pDataOut != NULL) && (pDataIn != NULL))
  {
    for (uint8_t i = 0, j, k; i < 8; i++, pDataIn += 6, pDataOut += 4)
    {
      j = (pDataIn[0] << 1) + pDataIn[5];
      k = (pDataIn[1] << 3) + (pDataIn[2] << 2) + (pDataIn[3] << 1) + pDataIn[4];

      for (uint8_t l = 0; l < 4; ++l)
      {
        pDataOut[l] = (S_Box[i][j][k] >> (3 - l)) & 1;
      }
    }
  }
}

static void Transform(uint8_t *pDataOut, uint8_t *pDataIn, const uint8_t *Table, uint16_t Length)
{
  uint8_t Tmp[256] = {0};

  if ((pDataOut != NULL) && (pDataIn != NULL) && (Table != NULL))
  {
    if ((Length > 0) && (Length <= 256))
    {
      for (uint16_t i = 0; i < Length; i++)
      {
        Tmp[i] = pDataIn[Table[i] - 1];
      }

      memcpy(pDataOut, Tmp, Length);
    }
  }
}

static void RotateL(uint8_t *pDataIn, uint16_t Length, uint16_t loop)
{
  // uint8_t Tmp[256] = {0};

  // if (pDataIn != NULL)
  // {
  //   if (Length != 0)
  //   {
  //     memcpy(Tmp, pDataIn, loop);
  //     memcpy(pDataIn, pDataIn + loop, Length - loop);
  //     memcpy(pDataIn + Length - loop, Tmp, loop);
  //   }
  // }

  if ((pDataIn == NULL) || (Length == 0) || (loop == 0) || (loop > Length))
    return;

  uint8_t temp[loop];
  memcpy(temp, pDataIn, loop);
  memmove(pDataIn, pDataIn + loop, Length - loop);
  memcpy(pDataIn + Length - loop, temp, loop);
}

static void ByteToBit(uint8_t *pDataOut, const uint8_t *pDataIn, uint16_t Bits)
{
  if ((pDataOut != NULL) && (pDataIn != NULL))
  {
    for (uint16_t i = 0; i < Bits; ++i)
    {
      pDataOut[i] = (pDataIn[i >> 3] >> ((7 - i) & 7)) & 1;
    }
  }
}

static void BitToByte(uint8_t *pDataOut, const uint8_t *pDataIn, uint16_t Bits)
{
  if ((pDataOut != NULL) && (pDataIn != NULL))
  {
    memset(pDataOut, 0, (Bits + 7) / 8);

    for (uint16_t i = 0; i < Bits; ++i)
    {
      pDataOut[i >> 3] |= pDataIn[i] << ((7 - i) & 7);
    }
  }
}

static void Xor(uint8_t *InA, const uint8_t *InB, uint16_t Length)
{
  if ((InA != NULL) && (InB != NULL))
  {
    if (Length != 0)
    {
      for (uint16_t i = 0; i < Length; i++)
      {
        InA[i] ^= InB[i];
      }
    }
  }
}

void DataXor(uint8_t *pDataIn, uint8_t *pDest, uint32_t DataSize, uint8_t *pDataOut)
{
  if ((pDataIn != NULL) && (pDest != NULL) && (pDataOut != NULL))
  {
    if (DataSize != 0)
    {
      for (uint32_t i = 0; i < DataSize; i++)
      {
        pDataOut[i] = pDest[i] ^ pDataIn[i];
      }
    }
  }
}

static void DesSetKey(const uint8_t *pKeyIn)
{
  uint8_t K[64] = {0}, *KL = &K[0], *KR = &K[28];

  if (pKeyIn != NULL)
  {
    ByteToBit(K, pKeyIn, 64);
    Transform(K, K, PC1_Table, 56);

    for (uint8_t i = 0; i < 16; i++)
    {
      RotateL(KL, 28, LoopTable[i]);
      RotateL(KR, 28, LoopTable[i]);
      Transform(SubKey[i], K, PC2_Table, 48);
    }
  }
}

static void Des(uint8_t *pDataOut, const uint8_t *pDataIn, uint8_t Type)
{
  uint8_t M[64] = {0}, Tmp[32] = {0}, *Li = &M[0], *Ri = &M[32];

  if ((pDataOut != NULL) && (pDataIn != NULL))
  {
    ByteToBit(M, pDataIn, 64);
    Transform(M, M, IP_Table, 64);

    if (Type == ENCRYPT)
    {
      for (uint8_t i = 0; i < 16; i++)
      {
        memcpy(Tmp, Ri, 32);
        F(Ri, SubKey[i]);
        Xor(Ri, Li, 32);
        memcpy(Li, Tmp, 32);
      }
    }
    else
    {
      for (uint8_t i = 0; i < 16; i++)
      {
        memcpy(Tmp, Ri, 32);
        F(Ri, SubKey[15 - i]);
        Xor(Ri, Li, 32);
        memcpy(Li, Tmp, 32);
      }
    }

    memcpy(Tmp, Li, 32);
    memcpy(Li, Ri, 32);
    memcpy(Ri, Tmp, 32);

    Transform(M, M, IPR_Table, 64);
    BitToByte(pDataOut, M, 64);
  }
}

void CalDesEncrypt(const uint8_t *pKeyIn, const uint8_t *pDataIn, uint8_t *pDataOut)
{
  DesSetKey(pKeyIn);
  Des(pDataOut, pDataIn, ENCRYPT);
}

void CalDesDecrypt(const uint8_t *pKeyIn, const uint8_t *pDataIn, uint8_t *pDataOut)
{
  DesSetKey(pKeyIn);
  Des(pDataOut, pDataIn, DECRYPT);
}

void CalTripleDesEcbEncrypt(const uint8_t *pKeyIn, const uint8_t *pDataIn, uint8_t *pDataOut)
{
  uint8_t KeyL[8] = {0}, KeyR[8] = {0}, TmpDest[8] = {0};

  if ((pKeyIn != NULL) && (pDataIn != NULL) && (pDataOut != NULL))
  {
    memcpy(KeyL, pKeyIn, 8);
    memcpy(KeyR, pKeyIn + 8, 8);

    CalDesEncrypt(KeyL, pDataIn, pDataOut);
    CalDesDecrypt(KeyR, pDataOut, TmpDest);
    CalDesEncrypt(KeyL, TmpDest, pDataOut);
  }
}

void CalcTripleDesEcbDecrypt(const uint8_t *pKeyIn, const uint8_t *pDataIn, uint8_t *pDataOut)
{
  uint8_t KeyL[8] = {0}, KeyR[8] = {0}, TmpDest[8] = {0};

  if ((pKeyIn != NULL) && (pDataIn != NULL) && (pDataOut != NULL))
  {
    memcpy(KeyL, pKeyIn, 8);
    memcpy(KeyR, pKeyIn + 8, 8);

    CalDesDecrypt(KeyL, pDataIn, pDataOut);
    CalDesEncrypt(KeyR, pDataOut, TmpDest);
    CalDesDecrypt(KeyL, TmpDest, pDataOut);
  }
}

void CalTripleDesCbcEncrypted(const uint8_t *pDataIn, uint32_t SizeIn, const uint8_t *pKeyValue, const uint8_t *pInitValue, uint8_t *pDataOut, uint32_t *pSizeOut)
{
  uint32_t ActualSize = 0;
  uint8_t DataBuff[1024] = {0};
  uint8_t KeyL[8] = {0}, KeyR[8] = {0}, TmpDest[8] = {0};
  uint8_t *pDataOffset = DataBuff, Remainder = 0, XorValue[8] = {0};

  if ((pDataIn != NULL) && (pKeyValue != NULL) && (pInitValue != NULL) && (pDataOut != NULL) && (pSizeOut != NULL))
  {
    if ((SizeIn != 0) && (SizeIn <= NOMINAL_PACKET_LENGTH))
    {
      Remainder = 8 - (SizeIn % 8);
      ActualSize = SizeIn + Remainder;

      memcpy(KeyL, pKeyValue, 8);
      memcpy(KeyR, pKeyValue + 8, 8);

      memcpy(pDataOffset, pDataIn, SizeIn);
      memset(&pDataOffset[SizeIn], Remainder, Remainder);

      memcpy(XorValue, pInitValue, 8);
      for (uint32_t i = 0; i < (ActualSize / 8); i++)
      {
        DataXor(pDataOffset, XorValue, 8, XorValue);

        CalDesEncrypt(KeyL, XorValue, pDataOut);
        CalDesDecrypt(KeyR, pDataOut, TmpDest);
        CalDesEncrypt(KeyL, TmpDest, pDataOut);

        memcpy(XorValue, pDataOut, 8);

        pDataOut += 8;
        pDataOffset += 8;
      }

      *pSizeOut = ActualSize;
    }
  }
}

void CalTripleDesCbcDecrypt(const uint8_t *pDataIn, uint32_t SizeIn, const uint8_t *pKeyValue, const uint8_t *pInitValue, uint8_t *pDataOut, uint32_t *pSizeOut)
{
  uint8_t *pDataOutOffset = pDataOut;
  uint8_t FillingLength = 0, XorValue[8] = {0};
  uint8_t KeyL[8] = {0}, KeyR[8] = {0}, TmpDest[8] = {0};

  if ((pDataIn != NULL) && (pKeyValue != NULL) && (pInitValue != NULL) && (pDataOut != NULL) && (pSizeOut != NULL))
  {
    if ((SizeIn != 0) && (SizeIn % 8) == 0)
    {
      memcpy(KeyL, pKeyValue, 8);
      memcpy(KeyR, pKeyValue + 8, 8);

      memcpy(XorValue, pInitValue, 8);
      for (uint32_t i = 0; i < (SizeIn / 8); i++)
      {
        CalDesDecrypt(KeyL, pDataIn, pDataOutOffset);
        CalDesEncrypt(KeyR, pDataOutOffset, TmpDest);
        CalDesDecrypt(KeyL, TmpDest, pDataOutOffset);

        DataXor(pDataOutOffset, XorValue, 8, pDataOutOffset);
        memcpy(XorValue, pDataIn, 8);

        pDataOutOffset += 8;
        pDataIn += 8;
      }

      FillingLength = pDataOut[SizeIn - 1];
      memset(&pDataOut[SizeIn - FillingLength], 0, FillingLength);
      *pSizeOut = SizeIn - FillingLength;
    }
  }
}

void Derive3DESKey(const uint8_t *pDataIn, const uint8_t *pKeyValue, uint8_t *pDataOut)
{
  if ((pDataIn != NULL) && (pKeyValue != NULL) && (pDataOut != NULL))
  {
    uint8_t leftData[8] = {0}, rightData[8] = {0}, modifyData[16] = {0};

    memcpy(leftData, pDataIn, 8);
    for (uint8_t i = 0; i < 8; i++)
    {
      rightData[i] = ~pDataIn[i];
    }

    CalTripleDesEcbEncrypt(pKeyValue, leftData, modifyData);
    CalTripleDesEcbEncrypt(pKeyValue, rightData, modifyData + 8);
    memcpy(pDataOut, modifyData, 16);
  }
}
